#include <WiFi.h>
#include <WebSocketsClient.h>
#include <Arduino.h>
#include <motor.h>  // 控制电机的头文件

// WiFi credentials
const char* ssid = "CMCC-Athlon";
const char* password = "haha1234";

// WebSocket client
WebSocketsClient webSocket;

// 当前的速度和角度
float current_linear_speed = 0.2; // 初始化前进速度
float current_angular_velocity = 0; // 初始角速度为0

// 电机速度更新函数
void updateMotorSpeed() {
    float linear_speed = current_linear_speed;  // 当前线速度
    float angular_velocity = current_angular_velocity;  // 当前角速度

    Serial.print("Updated speed... Linear: ");
    Serial.print(linear_speed);
    Serial.print(", Angular: ");
    Serial.println(angular_velocity);

    float v_left = linear_speed - (angular_velocity * WHEELBASE / 2.0);
    float v_right = linear_speed + (angular_velocity * WHEELBASE / 2.0);
    v_left = v_left / WHEEL_R;
    v_right = v_right / WHEEL_R;
    set_motor_speed(v_left, v_right);
}

// 处理接收到的 WebSocket 事件
void webSocketEvent(WStype_t type, uint8_t * payload, size_t length) {
    if (type == WStype_TEXT) {
        String message = String((char*)payload);
        Serial.println("Received message: " + message);

        // 根据接收到的消息进行控制
        if (message.indexOf("前进") >= 0) {
            current_linear_speed = 0.2; // 设置前进速度
            current_angular_velocity = 0; // 停止转向
        } else if (message.indexOf("后退") >= 0) {
            current_linear_speed = -0.2; // 设置后退速度
            current_angular_velocity = 0; // 停止转向
        } else if (message.indexOf("左转") >= 0) {
            current_linear_speed = 0; // 停止前进
            current_angular_velocity = 0.5; // 设置左转角速度
        } else if (message.indexOf("右转") >= 0) {
            current_linear_speed = 0; // 停止前进
            current_angular_velocity = -0.5; // 设置右转角速度
        } else {
            // 如果接收到的命令不匹配，可以设置为停止
            current_linear_speed = 0;
            current_angular_velocity = 0;
        }
        
        // 更新电机速度
        updateMotorSpeed();
    }
}

void setup() {
    Serial.begin(115200);
    Serial.println();
    Serial.print("Connecting to ");
    Serial.println(ssid);

    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED) {
        Serial.println("Connecting to WiFi...");
        delay(500);
        Serial.print(".");
    }
    Serial.println("");
    Serial.println("WiFi connected");
    Serial.println("IP address: ");
    Serial.println(WiFi.localIP());

    // 初始化 WebSocket 连接
    webSocket.begin("your-llm-server-ip", your-llm-server-port, "/");
    webSocket.onEvent(webSocketEvent);
}

void loop() {
    // 处理 WebSocket 事件
    webSocket.loop(); 

    // 更新电机速度（确保在每次循环中都调用）
    updateMotorSpeed();

    delay(10); // 延时以避免过高的循环速度
}